package es.uji.viselab.math;



/**
 * 
 * Class to store 6 joints. This provides basic functionality similar of a vector
 *
 */
public class Joints {
	private double joint1;
	private double joint2;
	private double joint3;
	private double joint4;
	private double joint5;
	private double joint6;

	public Joints() {
	}

	public Joints(double value1, double value2, double value3, double value4, double value5, double value6) {

		joint1 = value1;
		joint2 = value2;
		joint3 = value3;
		joint4 = value4;
		joint5 = value5;
		joint6 = value6;

	}

	public Joints(Joints j0) {
		joint1=j0.joint1;
		joint2=j0.joint2;
		joint3=j0.joint3;
		joint4=j0.joint4;
		joint5=j0.joint5;
		joint6=j0.joint6;
	}

	public Joints(double[] j) {
		joint1=j[0];
		joint2=j[1];
		joint3=j[2];
		joint4=j[3];
		joint5=j[4];
		joint6=j[5];
	}

	public Joints(RobotPosition pos) {
		joint1=pos.getJoint1();
		joint2=pos.getJoint2();
		joint3=pos.getJoint3();
		joint4=pos.getJoint4();
		joint5=pos.getJoint5();
		joint6=pos.getJoint6();
	}

	public double getJoint1() {
		return joint1;
	}

	public void setJoint1(double joint1) {
		this.joint1 = joint1;
	}

	public double getJoint2() {
		return joint2;
	}

	public void setJoint2(double joint2) {
		this.joint2 = joint2;
	}

	public double getJoint3() {
		return  joint3;
	}

	public void setJoint3(double joint3) {
		this.joint3 = joint3;
	}

	public double getJoint4() {
		return joint4;
	}

	public void setJoint4(double joint4) {
		this.joint4 = joint4;
	}

	public double getJoint5() {
		return joint5;
	}

	public void setJoint5(double joint5) {
		this.joint5 = joint5;
	}

	public double getJoint6() {
		return joint6;
	}

	public void setJoint6(double joint6) {
		this.joint6 = joint6;
	}

	public void setJoint(int i, double value) {
		switch (i) {
		case 1:
			setJoint1(value);
			break;
		case 2:
			setJoint2(value);
			break;
		case 3:
			setJoint3(value);
			break;
		case 4:
			setJoint4(value);
			break;
		case 5:
			setJoint5(value);
			break;
		case 6:
			setJoint6(value);
			break;
		}
	}

	public double getJoint(int i) {
		switch (i) {
		case 1:
			return getJoint1();
		case 2:
			return getJoint2();
		case 3:
			return getJoint3();
		case 4:
			return getJoint4();
		case 5:
			return getJoint5();
		case 6:
			return getJoint6();
		}
		return 0;
	}

	public double[] getArray() {
        double j[] = new double[6];
		j[0]=joint1;
		j[1]=joint2;
		j[2]=joint3;
		j[3]=joint4;
		j[4]=joint5;
		j[5]=joint6;
		return j;
	}

	public String getJointToString(int i, int precision) {
		double value = getJoint(i)*360 / (2*Math.PI);
		value = value * Math.pow(10, precision);
		value = (int)value;
		value = value / (Math.pow(10, precision));
		return String.valueOf(value);
	}


}